/*
 * Bounds.cpp
 *
 *  Created on: Nov 30, 2011
 *      Author: ECE_Guest
 */


#define LENGTH 4
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <WProgram.h>
#include "Bounds.h"
#include "Coordinate.h"

//using namespace std;

    Coordinate Origin (0,0);
    Coordinate bounds[LENGTH] = Coordinate(0,0);

Bounds::Bounds (double x, double y)
{
	//Coordinate o (x,y);
	Coordinate o (-45.0588,37.1076);
	Origin = o;
	createBoundsArray();
}



    void createBoundsArray ()
    {
    	Coordinate a (-44.0256,38.04);
    	Coordinate b (-43.3992,36.7362);
    	Coordinate c (-45.8724,35.9802);
    	Coordinate d (-46.7796,37.1076);
    	bounds[0] = a;
    	bounds[1] = b;
    	bounds[2] = c;
    	bounds[3] = d;
    }

    bool Bounds::inBounds (Coordinate test)
    {
    	Coordinate bounds1 (0,0);
    	Coordinate bounds2 (0,0);
    	int boundsCrossed = 0;
    	int i;
    	for(i=0; i<LENGTH; i++)
    	{
    		bounds1 = bounds[i];
    		if(i==LENGTH-1)
    		{
    			bounds2 = bounds[0];
    		}
    		else
    		{
    			bounds2 = bounds[i+1];
    		}

    		/*
    		* This "if" is to test whether the two lines are parallel, because
    		* if they are, their intersection will not be valid
			*/
    		if(true)
    		{
    			Coordinate intersect = getIntersection(test, bounds1, bounds2);
    			if(intersect.getLon()==bounds1.getLon() && intersect.getLat()==bounds1.getLat())
				{
					/*
					 * This if is put in place so that in the case that the line segemnt
					 * from origin to test point runs through a boundary point, that
					 * intersection will be counted only once and not twice. When this point
					 * assumes the role of bounds2 (every point gets to be both bounds1
					 * and bounds2 at some point) then this intersection will be counted.
					 * There is also the chance that the intersection of lines will occur on
					 * a boundary point with the test point inside the bounds. While this will
					 * return out of bounds, we will take multiple readings before deciding
					 * that the collar is truly out of bounds, and these multiple readings
					 * will each be slightly different.
					 */
				}
				else if(intersectionValid(intersect, test, bounds1, bounds2))
				{
					boundsCrossed++;
				}
    		}
    	}
    	///printf("Bounds Crossed: %d\n", boundsCrossed);
    	if(boundsCrossed%2 == 0)
		{
			return true;
		}

		return false;
    }

    /*
     * This method takes in 3 points (and can reference the origin point) and returns
     * the intersection of the line formed by the origin and test with the line formed
     * by the two bounds points
     */
	Coordinate Bounds::getIntersection (Coordinate test, Coordinate b1, Coordinate b2)
	{
		double x1,x2,x3,x4,y1,y2,y3,y4,x,y;

		/*
		 * There are five possible cases:
		 * 1) The boundary line is vertical, the test-origin line is horizontal
		 * 2) The boundary line is vertical, the test-origin line isn't horizontal
		 * 3) The test-origin line is vertical, the boundary line is horizontal
		 * 4) The test-origin line is vertical, the boundary line isn't horizontal
		 * 5) Neither line is vertical
		 *
		 * Here we deal with each of these cases. We don't care if the lines are parallel,
		 * because they will never intersect.
		 */
		if(b1.getLon()==b2.getLon())
		{
			/*
			 * Case 1: boundary vertical, test-origin horizontal
			 * Solution: we know the intersection easily
			 */
			if(Origin.getLat()==test.getLat())
			{
				x = b1.getLon();
				y = Origin.getLat();
			}

			/*
			 * Case 2: boundary vertical, test-origin not horizontal
			 * Solution: flip the axes so there are no vertical lines
			 */
			else
			{
				y1 = Origin.getLon();
				y2 = test.getLon();
				y3 = b1.getLon();
				y4 = b2.getLon();
				x1 = Origin.getLat();
				x2 = test.getLat();
				x3 = b1.getLat();
				x4 = b2.getLat();

				y = (x1*(y2-y1)/(x2-x1)-x3*(y4-y3)/(x4-x3)+y3-y1)/((y2-y1)/(x2-x1)-(y4-y3)/(x4-x3));
				x = (y2-y1)/(x2-x1)*(y-x1)+y1;
			}
		}
		else if(Origin.getLon()==test.getLon())
		{
			/*
			 * Case 3: test-origin vertical, boundary horizontal
			 * Solution: we know the intersection easily
			 */
			if(b1.getLat()==b2.getLat())
			{
				x = Origin.getLon();
				y = b1.getLat();
			}

			/*
			 * Case 4: test-origin vertical, boundary not horizontal
			 * Solution: flip axes so there are no vertical lines
			 */
			else
			{
				y1 = Origin.getLon();
				y2 = test.getLon();
				y3 = b1.getLon();
				y4 = b2.getLon();
				x1 = Origin.getLat();
				x2 = test.getLat();
				x3 = b1.getLat();
				x4 = b2.getLat();

				y = (x1*(y2-y1)/(x2-x1)-x3*(y4-y3)/(x4-x3)+y3-y1)/((y2-y1)/(x2-x1)-(y4-y3)/(x4-x3));
				x = (y2-y1)/(x2-x1)*(y-x1)+y1;
			}
		}

		/*
		 * Case 5: neither line is vertical
		 * Solution: use the intersection of two lines formula
		 */
		else
		{
			x1 = Origin.getLon();
			x2 = test.getLon();
			x3 = b1.getLon();
			x4 = b2.getLon();
			y1 = Origin.getLat();
			y2 = test.getLat();
			y3 = b1.getLat();
			y4 = b2.getLat();

			x = (x1*(y2-y1)/(x2-x1)-x3*(y4-y3)/(x4-x3)+y3-y1)/((y2-y1)/(x2-x1)-(y4-y3)/(x4-x3));
			y = (y2-y1)/(x2-x1)*(x-x1)+y1;
		}

		return Coordinate (x, y);
	}

	/*
	 * This method takes a given intersection of two lines and checks whether the intersection
	 * occurs within the two lines segments formed by the four points
	 */
	bool Bounds::intersectionValid (Coordinate intersection, Coordinate test, Coordinate b1, Coordinate b2)
	{
		double x1 = Origin.getLon();
		double x2 = test.getLon();
		double x3 = b1.getLon();
		double x4 = b2.getLon();
		double y1 = Origin.getLat();
		double y2 = test.getLat();
		double y3 = b1.getLat();
		double y4 = b2.getLat();
		double interX = intersection.getLon();
		double interY = intersection.getLat();

		/*
		 * A bit of craziness: just found a bug with this. If the line drawn from
		 * the origin to the test point intersects the bounds AT A BOUNDARY POINT,
		 * then the program will think it has crossed two lines and call it inbounds.
		 * I fixed this with an "if" statement in the "inbounds" method. It executes
		 * no code and has a comment explaining its purpose.
		 */

		if(x1>=x2)
		{
			if(!(x1>=interX && interX>=x2))
			{
				return false;
			}
		}
		else
		{
			if(!(x2>=interX && interX>=x1))
			{
				return false;
			}
		}
		if(x3>=x4)
		{
			if(!(x3>=interX && interX>=x4))
			{
				return false;
			}
		}
		else
		{
			if(!(x4>=interX && interX>=x3))
			{
				return false;
			}
		}


		if(y1>=y2)
		{
			if(!(y1>=interY && interY>=y2))
			{
				return false;
			}
		}
		else
		{
			if(!(y2>=interY && interY>=y1))
			{
				return false;
			}
		}
		if(y3>=y4)
		{
			if(!(y3>=interY && interY>=y4))
			{
				return false;
			}
		}
		else
		{
			if(!(y4>=interY && interY>=y3))
			{
				return false;
			}
		}
		return true;
	}

	double Bounds::shortestDistance (Coordinate test)
	{
		int i;
		double Ax, Bx, Cx, Ay, By, Cy, a, b, c, r;
		Coordinate bounds1(0,0);
		Coordinate bounds2(0,0);
		double tempDistance;
		double distance = 999999999;

		/*
		 * Iterate through sets of adjacent boundary points
		 */
		for(i=0; i<LENGTH; i++)
		{
			bounds1 = bounds[i];
			if(i==LENGTH-1)
			{
				bounds2 = bounds[0];
			}
			else
			{
				bounds2 = bounds[i+1];
			}
			Cx = test.getLon();
			Ax = bounds1.getLon();
			Bx = bounds2.getLon();
			Cy = test.getLat();
			Ay = bounds1.getLat();
			By = bounds2.getLat();

			/*
			 * r represents where the perpendicular intersection of the test point intersects the line segment formed by
			 * the two boundary points. If r<0 the intersection occurs outside of bounds1. If r>1 the intersection
			 * occurs outside of bounds2. If 0<r<1 the intersection occurs on the line segment.
			 */
			r = ((Cx-Ax)*(Bx-Ax)+(Cy-Ay)*(By-Ay))/pow(sqrt(pow((Bx-Ax),2)+pow((By-Ay),2)),2);

			if (0<r && r<1)
			{
				/*
				 * This tests whether the given boundary line segment is a vertical line. If it is, we swap all x and y
				 * coordinates for both boundary points and the test point. This allows us to sidestep the vertical line
				 * problem and calculate the accurate distance.
				 */
				if(Ax==Bx)
				{
					a = (Bx-Ax)/(By-Ay);
					b = -1;
					c = Ax - ((Bx-Ax)/(By-Ay))*Ay;
					Cx = test.getLat();
					Cy = test.getLon();
				}
				else
				{
					a = (By-Ay)/(Bx-Ax);
					b = -1;
					c = Ay - ((By-Ay)/(Bx-Ax))*Ax;
				}

				/*
				 * The absolute value function was truncating our doubles to integers, so we implemented our own here.
				 * We use the point-line distance formula.
				 */
				if(a*Cy+b*Cx+c >= 0)
				{
					tempDistance = (a*Cx+b*Cy+c)/sqrt(a*a+b*b);
				}
				else
				{
					tempDistance = -1*(a*Cx+b*Cy+c)/sqrt(a*a+b*b);
				}

				/*
				 * We only want to consider positive distances- otherwise the smallest distance will be the greatest
				 * negative number
				 */
				if(tempDistance < 0)
				{
					tempDistance *= -1;
				}

				if (distance>tempDistance)
				{
					distance=tempDistance;
				}
			}
			/*
			 * If the perpendicular intersection occurs outside of the line segment, we calculate the distance to
			 * each of the two boundary points.
			 */
			else
			{
				tempDistance = sqrt(pow((Cx-Ax),2)+pow((Cy-Ay),2));
				if (distance>tempDistance)
				{
					distance=tempDistance;
				}

				tempDistance = sqrt(pow((Cx-Bx),2)+pow((Cy-By),2));
				if (distance>tempDistance)
				{
					distance=tempDistance;
				}
			}

		}
		return distance;
	}


void Bounds::CheckPoint(double x, double y)
{
	Coordinate testing (x,y);
	///testing.print();

	double dist = shortestDistance(testing);
		///printf("Distance: %e\n",dist);
		bool isInBounds = inBounds(testing);
		if(isInBounds)
		{
			if(dist > 0.25)		// Inbounds and not within buffer zone: GREEN
			{
				digitalWrite(5, HIGH);
				digitalWrite(4, LOW);
				digitalWrite(3, LOW);
			}
			else				// Inbounds and within buffer zone: YELLOW
			{
				digitalWrite(5, LOW);
				digitalWrite(4, LOW);
				digitalWrite(3, HIGH);
			}
		}
		else					// Out of bounds: RED
		{
			digitalWrite(5, LOW);
			digitalWrite(4, HIGH);
			digitalWrite(3, LOW);
		}

		///outputToGUI(testing, isInBounds, dist);
}
